#include "main.h"
#include "mecanum.h"
#include "bsp_rc.h"
#include "remote_control.h"

Chassis_Speed chassis;
Wheel_Speed wheel;
float wheel_rpm[4];//代表四个轮子速度

/*以下的vx vy都以rpm为单位，*/
const Chassis_Speed *get_chassis_targetspeed(void)
{
    return &chassis;
}

const Wheel_Speed *get_wheel_speed(void)
{
    return &wheel;
}

void Data_Prepare(Chassis_Speed *chassis)
{
    chassis->vx = 2*get_remote_control_point()->rc.ch[3];//左摇杆上下，范围-660~660
    chassis->vy = 2*get_remote_control_point()->rc.ch[2];//左摇杆左右，范围-660~660
    chassis->vw = get_remote_control_point()->rc.ch[0];//右摇杆左右,定义-660为逆时针转，正660为顺时针转

}

void Mec_Cal(Chassis_Speed *chassis,Wheel_Speed *wheel)//这里相当于得到了目标速度
{
    Data_Prepare(chassis);

    /*麦轮速度解算*/
    // wheel->wheel_0 = chassis->vx - chassis->vy - chassis->vw;//左前轮，第一象限
    // wheel->wheel_1 = chassis->vx + chassis->vy - chassis->vw;//左后轮，第二象限
    // wheel->wheel_2 = chassis->vx - chassis->vy + chassis->vw;//右后轮，第三象限
    // wheel->wheel_3 = chassis->vx + chassis->vy + chassis->vw;//右前轮，第四象限

    wheel->wheel_0 = chassis->vx + chassis->vy + chassis->vw;//左前轮，第一象限
    wheel->wheel_1 = -chassis->vx - chassis->vy + chassis->vw;//左后轮，第二象限
    wheel->wheel_2 = -chassis->vx + chassis->vy + chassis->vw;//右后轮，第三象限
    wheel->wheel_3 = chassis->vx - chassis->vy + chassis->vw;//右前轮，第四象限
  
}



